#!/usr/bin/env python
"""
  It generates state machines for cleaning table
"""
import roslib; roslib.load_manifest("sushi_smach")
import rospy
import smach
import smach_ros
from sushi_smach import states,PickupStateGenerator

class TableCleanStateMachineGenerator:

  def generate(self,world,nav_states,base):

    pickup_sm = PickupStateGenerator.PickupStateGenerator().generate(world,nav_states,base)    
#nav_states = states.NavigateTo()

    sm = smach.StateMachine(outcomes=['success','failure'])

    T = {'failure':'failure'}
    
    with sm:
      # Step 0 : Start State
      smach.StateMachine.add('start_state',states.Start(),transitions = {'success':'clean_it'})
      
      # Step 1 : Call Tom's code
      smach.StateMachine.add('clean_it',states.CleanTable(),transitions = {'success':'success','failure':'failure'})

      """
      # Step 1 : Move to serving table
      T['success'] = 'scan_table'
      T['failure'] = 'failure'
      smach.StateMachine.add('move_to_servtable',nav_states['serv_table'],transitions = T.copy())

      # Step 2 : Scan Table and update World State
      smach.StateMachine.add('scan_table',states.ScanTable(world),transitions = {'no_object':'success','objects':'pickup'})

      # Step 3 : Pick up objects
      T['success'] = 'move_to_dirtytable'
#T['success'] = 'success'
      T['failure'] = 'failure'
      smach.StateMachine.add('pickup',pickup_sm,transitions = T.copy())

      # Step 3.5 : move to dirty table
      T['success'] = 'putdown'
      T['failure'] = 'failure'
      smach.StateMachine.add('move_to_dirtytable',nav_states['dirty_table'],transitions = T.copy())
      
      # Step 4 : Move and Put down
      T['success'] = 'move_to_servtable' 
      T['failure'] = 'failure'
      smach.StateMachine.add('putdown',states.Putdown(world),transitions = T.copy())
      """

    return sm
